#include "MotorControler.h"
#include "Motors.h"
#include <boost/bind.hpp>
#include <boost/function.hpp>
#include <cstdlib>
#include <iomanip>

MotorControler::MotorControler(Motors * ActuatorGroup,
		 int motor_id, 
		 double pid_coef_p, 
		 double pid_coef_i, 
		 double pid_coef_d, 
		 int pid_out_max, 
		 int opto_gpio, 
		 int opto_freq_div):
m_ActuatorGroup(ActuatorGroup),
m_motorId(motor_id),
m_Opto(new OptoCounter(opto_gpio, opto_freq_div)),
m_Pid(new PIDControler(boost::bind(&MotorControler::ImportInputValue, this), boost::bind(&MotorControler::ExportOutputValue, this, _1),m_Consigne, 0, pid_out_max, pid_coef_p, pid_coef_i, pid_coef_d))
{
	//force value to 0 at startup
	m_Consigne = 0;
	m_MotorSentPower = -1;

	m_Opto->Run();
}

MotorControler::~MotorControler()
{
	delete m_Opto;
	delete m_Pid;
}

double MotorControler::ImportInputValue()
{
	return m_Opto->m_NbToursParSeconds;
}

void MotorControler::ExportOutputValue(double NouvelleConsigne)
{
	if(static_cast<int>(NouvelleConsigne) == m_MotorSentPower)
		return;

	m_MotorSentPower = static_cast<int>(NouvelleConsigne);

	char SerialMsg[4] = { 0x80, 0x00, 0x00, 0x00};
	
	if(0 <= m_MotorSentPower) //positif
	{
		SerialMsg[2] = (m_motorId * 2) + 1;
		SerialMsg[3] = abs(m_MotorSentPower) & 0xFF;
	}
	else //negatif
	{
		SerialMsg[2] = (m_motorId * 2);
		SerialMsg[3] = abs(m_MotorSentPower) & 0xFF;
	}

	//std::cout << " serial write : " << std::setw(2) << std::hex << std::setfill('0') << static_cast<int>(SerialMsg[0]) << " " << std::setw(2) << std::hex << std::setfill('0') << static_cast<int>(SerialMsg[1]) << " " << std::setw(2) << std::hex << std::setfill('0') << static_cast<int>(SerialMsg[2]) << " " << std::setw(2) << std::hex << std::setfill('0') << static_cast<int>(SerialMsg[3]) << std::endl;
	m_ActuatorGroup->WriteToBus(SerialMsg, 4);
}

void MotorControler::SetMotorValue(double NouvelleConsigne)
{
	ExportOutputValue(NouvelleConsigne);
}

void MotorControler::Iteration()
{
	std::cout << "    Iterate Motor " << m_motorId << std::endl;
	m_Pid->Iteration();
}

